clear
close

global f b;
f = 400; b = 0.1;
P = 9; R = 0.09;
xMu = 20;

xTrue = 26;
yTrue = f*b/xTrue;
yMeas = yTrue - 0.6;

STEP = .1;
x = (10:STEP:35)';
px = normpdf(x, xMu,  sqrt(P));
plot(x, px, 'DisplayName', 'p(x)')

hold on

pyx = normpdf(yMeas, f*b./x, sqrt(R));
pxy = pyx .* px;
pxy = pxy / sum(pxy * STEP);
plot(x, pxy, 'DisplayName', 'p(x|y)')

xMap = x(pxy == max(pxy));
plot([xMap xMap],[0 0.25], 'DisplayName', 'x_{map}')

xMean = sum(x .* pxy * STEP);
plot([xMean xMean],[0 0.25], 'DisplayName', 'x_{mean}')

%EKF
xk = 20;
yk = yMeas;
Pk = P;
Rk = R; %todo Embedded Jacobian
Gk = G(xk);
Kk = Pk*Gk'*(Gk * Pk *Gk' + Rk)^-1;
xMu = xk + Kk * (yk - g(xk));
xCovariance = (1-Kk*Gk)*Pk;
pEkf = normpdf(x, xMu, sqrt(xCovariance));
plot(x, pEkf, 'DisplayName', 'x_{ekf}')

%IEKF
xk = 20;
yk = yMeas;
Pk = P;
Rk = R; %todo Embedded Jacobian
xop = xk;
for k = 1:10
    Gk = G(xop);
    Kk = Pk*Gk'*(Gk * Pk *Gk' + Rk)^-1;
    xop = xk + Kk * (yk - g(xop) - Gk * (xk - xop));
end
xMu = xop;
xCovariance = (1-Kk*Gk)*Pk;
pIekf = normpdf(x, xMu, sqrt(xCovariance));
plot(x, pIekf, 'DisplayName', 'x_{iekf}')

legend('-DynamicLegend');

function y = g(x)
    global f b;
    y = f*b/ x;
end

function Gk = G(x)
    global f  b;
    Gk = -f * b /(x^2);
end